package com.wltt.product.frfront.executors;

import com.wltt.product.frfront.config.exception.BusinessException;
import com.wltt.product.frfront.config.robot.WlttRobot;
import com.wltt.product.frfront.constants.RobotMoveConstant;
import com.wltt.product.frfront.constants.RobotPoseConstant;
import com.wltt.product.frfront.enums.CoordinateSystemEnum;
import com.wltt.product.frfront.holder.RobotHolder;
import com.wltt.product.frfront.model.vo.PoseMoveREQ;
import fairino.Robot;
import org.apache.logging.log4j.LogManager;
import org.apache.logging.log4j.Logger;
import org.springframework.http.HttpStatus;

public class PoseMoveExecutor implements Runnable {

    private static final Logger logger = LogManager.getLogger(PoseMoveExecutor.class);

    /**
     * 机器人对象
     */
    private WlttRobot robot;

    /**
     * 运动参数
     */
    private PoseMoveREQ poseMoveREQ;

    public PoseMoveExecutor(WlttRobot robot, PoseMoveREQ poseMoveREQ) {
        this.robot = robot;
        this.poseMoveREQ = poseMoveREQ;
    }

    @Override
    public void run() {
        if (!RobotHolder.setBusy()) {
            return;
        }
        if (!RobotHolder.setRobotMoving()) {
            return;
        }

        try {
            robot.Mode(0);

            int refType;
            if (CoordinateSystemEnum.COORDINATE_SYSTEM_BASE.getCode() == poseMoveREQ.getCoordinateSystemType()) {
                refType = 2;
            } else if (CoordinateSystemEnum.COORDINATE_SYSTEM_TOOL.getCode() == poseMoveREQ.getCoordinateSystemType()) {
                refType = 4;
            } else if (CoordinateSystemEnum.COORDINATE_SYSTEM_WORKPIECE.getCode() == poseMoveREQ.getCoordinateSystemType()) {
                refType = 8;
            } else {
                throw new BusinessException("坐标系无效", HttpStatus.BAD_REQUEST);
            }

            int nb;
            if (RobotPoseConstant.POSE_X.equalsIgnoreCase(poseMoveREQ.getDescTran())) {
                nb = 1;
            } else if (RobotPoseConstant.POSE_Y.equalsIgnoreCase(poseMoveREQ.getDescTran())) {
                nb = 2;
            } else if (RobotPoseConstant.POSE_Z.equalsIgnoreCase(poseMoveREQ.getDescTran())) {
                nb = 3;
            } else if (RobotPoseConstant.POSE_RX.equalsIgnoreCase(poseMoveREQ.getDescTran())) {
                nb = 4;
            } else if (RobotPoseConstant.POSE_RY.equalsIgnoreCase(poseMoveREQ.getDescTran())) {
                nb = 5;
            } else if (RobotPoseConstant.POSE_RZ.equalsIgnoreCase(poseMoveREQ.getDescTran())) {
                nb = 6;
            } else {
                throw new BusinessException("指定关节无效", HttpStatus.BAD_REQUEST);
            }

            int dir;
            if (RobotMoveConstant.FORWARD == poseMoveREQ.getDirection()) {
                dir = 1;
            } else if (RobotMoveConstant.REVERSE == poseMoveREQ.getDirection()) {
                dir = 0;
            } else {
                throw new BusinessException("指定方向无效", HttpStatus.BAD_REQUEST);
            }

//            int step = 0;
//            while (RobotHolder.isRobotMoving() && step < poseMoveREQ.getMaxStep()) {
//                int result = robot.StartJOG(refType, nb, dir, 30, 80, poseMoveREQ.getMaxStep());//关节点动
//                if (result != 0) {
//                    throw new BusinessException("运动指令执行失败", HttpStatus.BAD_REQUEST);
//                }
//                step = step + 5;
//                Thread.sleep(500);
//            }
            robot.StartJOG(refType, nb, dir, 5, 50, poseMoveREQ.getMaxStep());//关节点动
        } finally {
            RobotHolder.setNoBusy();
            RobotHolder.setRobotNoMoving();
            robot.Mode(1);
        }
    }
}
